#include "LcGraphCut.h"

void superResReconstruct(Mat & res, Mat & label_map, Mat & dst)
{
	int _rows = label_map.rows;
	int _cols = label_map.cols;

	dst = Mat::zeros( _rows, _cols , 5 );

	int n_node = res.rows;

	Mat lab = Mat::zeros( n_node, 1, 5);

	for(int i = 0 ; i < n_node ; i++)
		if( res.at<float>( i, 0) > 0.5)
			lab.at<float>(i,0) = 1.0;
		else
			lab.at<float>(i,0) = 0.0;


	for(int y= 0 ; y< _rows; y++)
		for(int x = 0 ;  x < _cols ; x++)
		{
			int id = label_map.at<int>(y,x);

			dst.at<float>(y,x) = lab.at<float>(id, 0);
		}

}

static void decodeMoment( LcColorMoment & node,Mat & mu , Mat & cov)
{	
	double * m;
	double * p;

	m = new(double[3]);
	p = new(double[9]);

	node.get_Color_Kernel( m, p);

	mu = Mat::zeros(3,1,CV_64F);
	cov = Mat::zeros(3,3,CV_64F);

	double * p1 = m;
	double * p2 = mu.ptr<double>();

	for(int i = 0 ; i < 3; i++, p1++,p2++)
		*p2 = *p1;

	//delete m;

	double * p3 = p;
	double * p4 = cov.ptr<double>();

	for(int i = 0 ; i < 9; i++, p3++,p4++)
		*p4 = *p3;

	//delete p;
}


NodeStatistic::NodeStatistic()
{
	sum_likelihood = 0; n = 0;	

	color_moment = LcColorMoment();

	sum_position_prior = 0;
}

EdgeStatistic::EdgeStatistic()
{
	len = 0;
	sum_Pb = 0;
}

static void keepThreeBigHeatMap(Mat & src, Mat & dst)
{

	Mat _src;

	if(src.type() == CV_8U)
	{
		src.convertTo( _src, 5 );
		_src = _src * (1/255.0);
	}
	else
		_src = src;

	Mat bin;		
	threshold(_src, bin,0.50,255,THRESH_BINARY);

	bin.convertTo(bin,CV_8UC1);

	Mat vis;
	cv::cvtColor( bin, vis, CV_GRAY2RGB);

	

	vector<vector<Point> > co;
	vector<Vec4i> hi;

	findContours(bin,co,hi,CV_RETR_EXTERNAL,CV_CHAIN_APPROX_NONE);

	

	int _rows = bin.rows;
	int _cols = bin.cols;

	dst = Mat::zeros( _rows, _cols , 5);

	{
		vector<float> sort_score;

		sort_score.resize( co.size());

		for(int i = 0 ; i < (int) co.size() ; i++)
			sort_score[i] = float(- contourArea( Mat( co[i] )) );

		vector<int> sort_id;

		lc::argsort( sort_score, sort_id );

		for(int i = 0 ; i < min(3,(int) sort_id.size()); i++)
		{
			int id = sort_id[i];

			if( - sort_score[id]<1000) continue;

			//get center

			Mat mask_new = Mat::zeros( _rows, _cols , CV_8U);

			drawContours( mask_new ,co,id,Scalar::all(255),-1,CV_AA);

			LcCoorMoment coor_moment;

			for(int y = 0; y < _rows; y++)
				for(int x = 0; x < _cols ; x++)
					if(mask_new.at<uchar>(y,x) > 100 )
						coor_moment.Add( double(x), double(y) );					

			{//append heat map
				double center_x = coor_moment.GetGX();
				double center_y = coor_moment.GetGY();

				double * I = new(double[4]);
				coor_moment.get_Inertia_Matrix( I );

				//( l-a)(l-c) - bd
				//l^2 - (a+c) l +ac-bd
				double B = (I[0] + I[3])/2.0;
				double C = I[0] * I[3] - I[1] * I[2];
				double D = sqrt(B*B - C);

				double l1 = B+D;
				double l2 = B-D;

				Point2d v1( I[1] , l1 -I[0]), v2( I[1], l2-I[0]);				

				v1 = v1*(1.0/ sqrt(v1.x*v1.x+v1.y*v1.y));
				v2 = v2*(1.0/ sqrt(v2.x*v2.x+v2.y*v2.y));

				double partition = 1.0/2.0/3.141592654/sqrt(l1*l2);

				double log_partition = log( partition);

				l1 *= 2.0;
				l2 *= 2.0;

				for(int y = 0; y < _rows; y++)
					for(int x = 0; x < _cols ; x++)
					{
						Point2d _v = Point2d( double(x)-center_x, double(y)-center_y);

						double p1 = _v.x * v1.x + _v.y * v1.y;
						double p2 = _v.x * v2.x + _v.y * v2.y;
						double val = p1 * p1 / l1 + p2* p2 /l2;

						val = sqrt(val);

						if(val> 1.5) val = 1.5;

						//val = -val / 2.0 + log_partition;
						//p(r) = 1.5 - val
						//P(r) = int(  (1.5-r)r dr 
						//dst.at<float>(y,x) += (1.5-val);//exp(val);

						dst.at<float>(y,x) += float(1 - val*val*val/1.5/1.5/1.5);

					}

			}//end append heat map
			
		}

	}

	if(0)
	{
		lc::colorshow("debug_heat_map",dst);

		imshow("debug",vis);
		cv::waitKey(0);
	}

	

}

void NodeStatistic::addPoint(Vec3b & color, float likelihood)
{
	Vec3d double_color = Vec3d( color );
	double_color = double_color / 255.0;
	color_moment.Add(double_color[0],double_color[1],double_color[2]);

	sum_likelihood+=likelihood;
	n+=1.0;
}

//================================

void LcMRF::getParameter( Mat & para )
{	
	{
		int _n = lambda[0].rows + lambda[1].rows;

		if(IF_NODE_BIAS) _n++;
		if(IF_EDGE_BIAS) _n++;

		para = Mat::zeros( _n , 1, 5 );
	}

	float * p_data = para.ptr<float>();

	for(int k =0 ; k < 2; k++)
	{
		float * p_dst = lambda[k].ptr<float>();

		for(int n = lambda[k].rows ; n>0; n--, p_dst++, p_data++)
			*p_data = *p_dst;
	}

	if(IF_NODE_BIAS)
	{
		*p_data = node_bias; p_data++;
	}

	if(IF_EDGE_BIAS)
	{
		*p_data = edge_bias; p_data++;
	}
}

void LcMRF::setParameter( Mat & para )
{
	float * p_data = para.ptr<float>();

	for(int k =0 ; k < 2; k++)
	{
		float * p_dst = lambda[k].ptr<float>();

		for(int n = lambda[k].rows ; n>0; n--, p_dst++, p_data++)
			*p_dst = *p_data;
	}

	if(IF_NODE_BIAS)
	{
		node_bias = *p_data; p_data++;
	}

	if(IF_EDGE_BIAS)
	{
		edge_bias = *p_data; p_data++;
	}
}

LcMRF::LcMRF()
{
	IF_NODE_BIAS = true;
	IF_EDGE_BIAS = true;
}

void LcMRF::set( vector< int > dims)
{
	lambda.resize( 2 );

	lambda[0] = Mat::zeros( dims[0], 1 , 5);
	lambda[0].at<float>(0,0) = 1.0f;

	lambda[1] = Mat::zeros( dims[1], 1 , 5);
	lambda[1].at<float>(0,0) = 0.1f;

	node_bias = 0.0f;
	edge_bias = 0.0f;
}

LcValidator LcMRF::predict(	Mat & node_desc, 
							Mat & edge_desc,
							Mat & edge_map,
							Mat & res,
							Mat & label)
{
	int n_node = node_desc.rows;	

	if(edge_map.type() == CV_32F)
		edge_map.convertTo( edge_map, CV_32S);

	Mat node_weight = node_desc * lambda[0];	

	if(IF_NODE_BIAS) node_weight = node_weight + node_bias;

	Mat edge_weight = edge_desc * lambda[1];

	if(IF_EDGE_BIAS) edge_weight = edge_weight + edge_bias;

	res = Mat::zeros( n_node , 1 , 5 );
	

	{
		float MAX_EDGE_CAP = 1000;

		typedef Graph< float , float , float > GraphType;

		GraphType * _g = new GraphType( n_node, n_node * 20 );

		for(int i = 0 ; i < n_node ; i ++)
		{
			//if(i>limit) continue;
			_g->add_node();
			_g->add_tweights( i , node_weight.at<float>(i,0) + MAX_EDGE_CAP, MAX_EDGE_CAP );
		}		

		for(int i = 0 ; i < edge_map.rows; i++)
		{			
			int id1 = edge_map.at<int>(i,0);
			int id2 = edge_map.at<int>(i,1);			
			
			//if(id1>=id2) continue;
			if(edge_weight.at<float>(i,0)<1e-4) continue;
			//_g->add_edge( id1, id2, edge_weight.at<float>(i,0), edge_weight.at<float>(i,0));
			_g->add_edge( id1, id2, edge_weight.at<float>(i,0), 0 );
		}

		float flow = _g->maxflow();			

		for(int i = 0 ; i < n_node ; i ++)
		{
			if( _g->what_segment(i) == GraphType::SOURCE) res.at<float>(i,0) = 1.0f;
			else res.at<float>(i,0) = 0.0f;
		}

		delete _g;
	}

	if( label.rows == res.rows ) return LcValidator( res, label);
	else return LcValidator();
}

//======================================

LcLMOpt::LcLMOpt()
{	
	jac_down_rate = 40;
}


LcLMOpt::LcLMOpt( 	vector<Mat> & n_node_descs,
					vector<Mat> & n_edge_descs,
					vector<Mat> & n_edge_maps,	
					vector<Mat> & n_labels)
{	

	jac_down_rate = 10;
	learning_rate = 0.1;
	cooling_rate = 0;
	iter_times = 100;


	set( n_node_descs, n_edge_descs, n_edge_maps, n_labels );
}

void LcLMOpt::set(	vector<Mat> & n_node_descs,
					vector<Mat> & n_edge_descs,
					vector<Mat> & n_edge_maps,	
					vector<Mat> & n_labels)
{
	node_descs = n_node_descs;
	edge_descs = n_edge_descs;
	edge_maps = n_edge_maps;
	labels = n_labels;

	n_frame = (int) labels.size();

	if( jac_down_rate> n_frame ) jac_down_rate = n_frame;
}

void LcLMOpt::calcJacobian( Mat & jac)
{	
	my_parameter.convertTo( my_parameter, CV_32F );

	int dim = my_parameter.rows;

	//float step = 2e-2f;

	jac = Mat::zeros( dim, 1, 5);

	int start = rand() % jac_down_rate;

	float score_init = calcScore(jac_down_rate, my_parameter, start);

	for(int i = 0 ; i < dim ; i ++)
	{		

		for(float step = 1e-3f; step< 0.2f; step*=1.5f)
		{
			Mat pos, neg;
			my_parameter.copyTo(pos);
			my_parameter.copyTo(neg);

			pos.at<float>(i,0) += step;
			neg.at<float>(i,0) -= step;		

			float score_pos = 1.0f - calcScore(jac_down_rate, pos, start);
			float score_neg = 1.0f - calcScore(jac_down_rate, neg, start);		

			if( score_pos< score_init && score_neg < score_init )
			{
				jac.at<float>(i,0) = (score_pos -score_neg) / 2.0f / step;
				//break;
			}
			else if(score_pos < score_init)
			{
				jac.at<float>(i,0) = (score_pos -score_init) / step * 2.0f;
				break;
			}
			else if(score_neg < score_init)
			{				
				jac.at<float>(i,0) = (score_init -score_neg) / step * 2.0f;
				break;
			}
			else
				jac.at<float>(i,0) = (score_pos -score_neg) / 2.0f / step * 0.5f;
		}
	}

	jac = jac.t();

}

void LcLMOpt::calcErr( Mat & err )
{
	Mat params;
	my_parameter.convertTo( params, CV_32F );

	float score = 1.0f - calcScore(1, params);
	err = Mat::zeros(1,1,5);
	err.at<float>(0,0) = 1.0f - score;
}

float LcLMOpt::calcScore( int down_rate, Mat & para, int start)
{		

	//para.convertTo( para, 5 );

	if( start < 0) start = rand() % down_rate;

	if(start > n_frame -1 ) start = n_frame -1;

	LcValidator _validator(0,0,0,0);

	my_MRF->setParameter( para );

	for(int i = start ; i <  n_frame ; i += down_rate)
	{		
		Mat res;		

		_validator = _validator 
			+ my_MRF->predict(	node_descs[i],
								edge_descs[i],
								edge_maps[i],
								res,
								labels[i]);						
	}	

	return _validator.getF1();
}

void LcLMOpt::work()
{
	CvTermCriteria term_criteria_ = CvTermCriteria();

	int dim = my_parameter.rows;

	CvLevMarq solver(dim,
                     1,
                     term_criteria_);

	Mat err, jac;
	
	//my_parameter.convertTo( my_parameter, CV_64F );

    //CvMat matParams = my_parameter;
    //cvCopy(&matParams, solver.param);	

	float F_max = calcScore(1, my_parameter);//Energy

	Mat my_parameter_max;

	my_parameter.copyTo(my_parameter_max);

	float temperature = 0.1f;

	for(int i = 0 ; i < iter_times; i++)
	{
		double step = learning_rate
			* (1 - cooling_rate * double(i+1) / double(iter_times));

		calcJacobian(jac);

		{
			//norm jac as a direction

			Mat temp = abs(jac);

			float norm = 1.0f/lc::reduceMat( temp, CV_REDUCE_SUM);

			jac = jac * norm;
		}

		{

			int _dim = jac.cols;
			vector<float> score; score.resize(_dim );
			for(int i = 0 ; i < _dim ;i++)
				score[i] = -abs( jac.at<float>( 0, i ) );

			vector<int> id;
			lc::argsort( score, id);

			for(int i = 0; i < _dim ; i++)
				if(rand()/double(RAND_MAX) < double(i)/double(_dim)* 0.7 + 0.3)
					jac.at<float>(0,id[i]) = float(lc::normrnd()) * temperature * 0.1f;
		}

		my_parameter = my_parameter - jac.t() * step;

		if(1){
			Mat temp = abs(my_parameter);
			float norm = 1.0f/lc::reduceMat( temp, CV_REDUCE_SUM);
			my_parameter = my_parameter * norm;
		}

		float F_new = calcScore(1, my_parameter);

		if(F_new > F_max)
		{
			F_max = calcScore(1, my_parameter);//Energy	

			my_parameter.copyTo(my_parameter_max);
		}
		else if( rand()/double(RAND_MAX) > exp( -(F_max - F_new)/temperature))
		{
			my_parameter_max.copyTo(my_parameter);
		}

		temperature*=0.97f;

		if(i%5==0) cout << "F= " << calcScore(1, my_parameter) << endl;;
	}	

	my_parameter_max.copyTo(my_parameter);
}

//======================================

LcGraphCut::LcGraphCut()
{
	channel_code[0] = 'l';
	channel_code[1] = 'G';
	channel_code[2] = 'L';

	compact_parameter = 20;

	n_superpixel = 600;

	expand_times = 0;

	visual = 0;

	node_computer.clear();
	edge_computer.clear();

}

void LcGraphCut::output(string prefix)
{

}

void LcGraphCut::getLabel( Mat & gt, Mat & label)
{
	Mat _gt;

	if(gt.type() == CV_8UC3) cv::cvtColor(gt,gt,CV_RGB2GRAY);

	if(gt.type() == CV_8U)
	{
		gt.convertTo( _gt, 5 );
		_gt = _gt *(1.0/255.0);
	}
	else _gt = gt;


	vector< int > count;

	int max_label;
	{
		Mat temp;
		label_map.convertTo( temp , 5);
		max_label = 1 + int(lc::reduceMat( temp ,CV_REDUCE_MAX));
	}

	count.resize( max_label );
	for(int i = 0 ; i < max_label ; i++) count[i] = 0;

	vector<float> sum_weight;
	sum_weight.resize( max_label );
	for(int i = 0 ; i < max_label ; i++) sum_weight[i] = 0;

	int _rows = gt.rows;
	int _cols = gt.cols;

	
	for(int y = 0; y< _rows; y++ )
		for(int x = 0 ; x< _cols; x++)
		{
			int id = label_map.at<int>(y,x);

			sum_weight[id]+= _gt.at<float>(y,x);

			count[id]++;
		}

	label = Mat::zeros( n_node, 1, 5);

	if(filter_map.size()==0)
	{
		for(int i = 0 ; i < n_node ; i++)
			label.at<float>( i, 0) = sum_weight[i] / float( count[i] );
	}
	else
	{
		for(int i = 0 ; i < n_node ; i++)
		{
			int id = filter_map[i];
			if(id>=0)
			label.at<float>( id, 0) = sum_weight[i] / float( count[i] );
		}
	}
}

void LcGraphCut::balanceBackGround(Mat & likelihood)
{
	Mat label;

	getLabel( likelihood , label );

	for(int i = 0; i < label.rows; i++)
		if(label.at<float>(i,0)>.5)  label.at<float>(i,0) = 1.0f;
		else label.at<float>(i,0) = 0;

	Mat temp;
	adj_mat.convertTo( temp,5);
	temp = temp + Mat::eye( temp.size(), 5);

	for(int i = 0 ; i < max(1,2 - expand_times); i ++)
		label = temp * label;

	vector<int> sel_id; sel_id.clear(); sel_id.reserve( label.rows );
	filter_map.resize( label.rows );
	for(int i = 0 ; i < label.rows; i++) filter_map[i] = -1;

	{
		
		int j = 0;
		for(int i = 0; i < label.rows; i++)
			if( label.at<float>(i,0) > .5 )
			{
				filter_map[i] = j;
				sel_id.push_back( i );
				node_statistic[j] = node_statistic[i];

				j++;
			}

		cout << j << " / " << label.rows << " rest" << endl;

		node_statistic.resize( j);
		Mat temp;
		lc::filterMat( adj_mat, temp , sel_id);

		temp.copyTo( adj_mat );

		n_node = j;
	}

	

}

void LcGraphCut::collectPb( Mat & Pb_result )
{
	for(int i = 0 ; i < n_node-1;i++)
		for(int j = 0 ; j < n_node; j++)
			if(adj_mat.at<int>(i,j) > 0 && i!=j && edge_statistic[i][j] == NULL) 
				edge_statistic[i][j] = new EdgeStatistic;

	Mat _Pb;

	if(Pb_result.type()==CV_8U)
	{
		Pb_result.convertTo( _Pb, 5);
		_Pb = _Pb * (1.0/255.0);
	}
	else _Pb = Pb_result;

	cv::GaussianBlur( _Pb, _Pb, Size(11,11),3,3);	

	const int dx8[8] = {-1, -1,  0,  1, 1, 1, 0, -1};
	const int dy8[8] = { 0, -1, -1, -1, 0, 1, 1,  1};

	int _rows = label_map.rows;
	int _cols = label_map.cols;

	for(int y = 1; y< _rows-1; y++ )
		for(int x = 1 ; x< _cols-1; x++)
		{
			int id1 = label_map.at<int>(y,x);

			for( int k = 0 ; k < 8; k++)
			{
				int x2 = x + dx8[k];
				int y2 = y+ dy8[k];

				int id2 = label_map.at<int>( y2, x2);

				float val = _Pb.at<float>( y,x ) +_Pb.at<float>( y2,x2 );

				if(edge_statistic[id1][id2]!=NULL)
				{
					EdgeStatistic * edge = edge_statistic[id1][id2];
					edge->len += 1.0f;
					edge->sum_Pb += val;
				}

				if(edge_statistic[id2][id1]!=NULL)
				{
					EdgeStatistic * edge = edge_statistic[id2][id1];
					edge->len += 1.0f;
					edge->sum_Pb += val;
				}
			}
		}
}

void LcGraphCut::collectHeatmap(Mat & likelihood)
{
	Mat _likelihood;
	medianBlur( likelihood, _likelihood, 5 );

	Mat heat_map;

	keepThreeBigHeatMap( _likelihood, heat_map );

	int _rows = heat_map.rows;
	int _cols = heat_map.cols;

	for(int y = 0; y< _rows; y++ )
		for(int x = 0 ; x< _cols; x++)
		{
			int id = label_map.at<int>(y,x);

			node_statistic[id].sum_position_prior
				+= heat_map.at<float>(y,x);
		}

}

void LcGraphCut::collectNodeStatistic( Mat & frm, Mat & likelihood)
{

	Mat _likelihood;

	if(likelihood.type() == CV_8U)
	{
		likelihood.convertTo( _likelihood, 5 );
		_likelihood = _likelihood *(1.0/255.0);
	}
	else _likelihood = likelihood;

	//label_map
	node_statistic.clear();

	node_statistic.reserve( n_node + 5 );	
	
	node_statistic.resize(n_node);	

	for(int i = 0 ; i < n_node;i++)
		node_statistic[i] = NodeStatistic();

	edge_statistic.resize(n_node);

	for(int i = 0 ; i < n_node ; i++)
	{
		edge_statistic[i].resize(n_node);

		for(int j = 0; j < n_node; j++)
			edge_statistic[i][j] = NULL;
	}

	int _rows = frm.rows;
	int _cols = frm.cols;

	
	for(int y = 0; y< _rows; y++ )
		for(int x = 0 ; x< _cols; x++)
		{
			int id = label_map.at<int>(y,x);

			Vec3b & color = frm.at<Vec3b>( y, x);

			float likeli = _likelihood.at<float>( y, x);

			node_statistic[id].addPoint( color, likeli);
		}

	
	if(0){
		Mat score_mat = getBinayScoreMat(label_map, adj_mat, new SpGaussTest);

		score_mat = max(score_mat,1.2+1e-5);

		score_mat = score_mat-1.2;

		cv::log(score_mat, score_mat);

		score_mat = min(score_mat,6);

		score_mat = max(score_mat,0);

		visualLinkAmplitude( label_map, adj_mat, score_mat, &frm);
	}
}

void LcGraphCut::expandAdjancencyMatrix( Mat & adj_mat , int expand_times)
{
	
	if(expand_times>0)
	{
		int _n = adj_mat.rows;

		Mat _temp = adj_mat + Mat::eye( _n , _n , CV_32S);

		Mat _ans;
		_temp.copyTo(_ans);

		for(int times = 0 ; times < expand_times ; times++)
		{
			Mat _n_ans = Mat::zeros( _n, _n , CV_32S);

			for(int i = 0 ; i < _n; i++)
				for(int k = 0 ; k<_n; k++)
					for(int j = 0 ; j <_n;j++)
					{
						if( _temp.at<int>(j,k) && _ans.at<int>(i,j))
						{
							_n_ans.at<int>(i,k) = 1;
							break;
						}
					}

			_n_ans.copyTo(_ans);
		}

		_ans = _ans - Mat::eye( _n , _n , CV_32S);

		_ans.copyTo( adj_mat );
	}
}

void LcGraphCut::set( Mat & frm, Mat & likelihood )
{
	setEdgeComputer();
	setNodeComputer();

	Mat likeli_merge;

	mergeLikeli( frm, likelihood,channel_code, likeli_merge );		

	getSLIC(likeli_merge,label_map);

	if(visual)
	{
		Mat dsp = showSLIC(frm, label_map);
		imshow("debug_dsp",dsp);
		cv::waitKey(10);
	}

	adj_mat = getAdjancencyMatrix( label_map, n_node);

	expandAdjancencyMatrix( adj_mat , expand_times);

	if(visual && 0)
		visualLink( label_map, adj_mat, n_node);

}

Mat LcGraphCut::getBinayScoreMat( Mat & label_map, Mat & adj_mat , EdgeFeatureExtractor * computer)
{
	int n_node = (int) node_statistic.size();

	if(n_node==0)
		{	cout << "visual Amplitude should after collect " << endl; return Mat();}

	vector<Point2f> node_center = getNodeCenter( label_map, n_node);	

	Mat score_mat = Mat::zeros( n_node, n_node, 5);
	
	EdgeStatistic empty;
	for(int i = 0 ; i < n_node-1;i++)
		for(int j = 0 ; j < n_node; j++)
			if(adj_mat.at<int>(i,j) > 0 )
			{
				Mat desc = score_mat( Rect(i,j,1,1));
				computer->compute( node_statistic[i], node_statistic[j] ,empty, desc);
			}

	return score_mat;
}

void LcGraphCut::visualLinkAmplitude(
						Mat & label_map,
						Mat & adj_mat ,
						Mat & score_mat,
						Mat * source)
{
	int n_node = score_mat.rows;

	vector<Point2f> node_center = getNodeCenter( label_map, n_node);

	Mat dsp;
	
	if(source==NULL)
		dsp = Mat::zeros( label_map.size(), CV_8UC3 );
	else
		source->copyTo(dsp);

	for(int i = 0 ; i < n_node ; i ++)
		circle(dsp, node_center[i] ,2,CV_RGB( 0,0,255),2,CV_AA);


	float min = 0.0f;
	float max = lc::reduceMat( score_mat, CV_REDUCE_MAX);

	score_mat = (score_mat-min)/(max-min);

	for(int i = 0 ; i < n_node-1;i++)
		for(int j = 0 ; j < n_node; j++)
		{
			float val = score_mat.at<float>(i,j);

			if(val > 1e-3 )
			{				
				Mat hsv = Mat::zeros(1,1,CV_8UC3);
				Mat rgb;

				val = val * 255.0f * .85f;
				hsv.at<Vec3b>(0,0) = Vec3b( (unsigned char) val,
											255,
											255);
				
				cv::cvtColor( hsv,rgb, CV_HSV2RGB_FULL);				

				unsigned char * p = rgb.ptr<unsigned char>();

				line(dsp,node_center[i],
						 node_center[j],CV_RGB( *p,*(p+1) , *(p+2) ),1,CV_AA);
			}
		}
		

	
	
	imshow("debug",dsp);
	cv::waitKey(1);
}

void LcGraphCut::visualLink( Mat & label_map, Mat & adj_mat ,int n_node)
{
	vector<Point2f> node_center = getNodeCenter( label_map, n_node);

	Mat dsp = Mat::zeros( label_map.size(), CV_8UC3 );

	for(int i = 0 ; i < n_node ; i ++)
	{
		circle(dsp, node_center[i] ,2,CV_RGB( 0,0,255),2,CV_AA);
	}

	for(int i = 0 ; i < n_node-1;i++)
		for(int j = 0 ; j < n_node; j++)
			if(adj_mat.at<int>(i,j) > 0 )
			{
				Point2f diff = node_center[i]-node_center[j];
				if(1 || abs(diff.x) + abs(diff.y) > 100)
				{
					line(dsp,node_center[i],
						 node_center[j],CV_RGB(rand()%255,rand()%255,rand()%255),1,CV_AA);
				}
			}

	imshow("debug",dsp);
	cv::waitKey(1);



}

vector<Point2f> LcGraphCut::getNodeCenter(Mat & label_map , int n_node)
{
	int _rows = label_map.rows;
	int _cols = label_map.cols;

	vector<Point2f> ans;
	ans.clear(); ans.reserve( n_node );
	for(int i = 0 ; i < n_node; i++) ans.push_back(Point2f(0,0));

	float * counts = new(float[n_node]);
	memset( counts, 0 ,  sizeof(float) * n_node );

	for(int y = 0; y< _rows; y++ )
		for(int x = 0 ; x< _cols; x++)
		{
			int id = label_map.at<int>(y,x);
			ans[id] = ans[id] + Point2f(float(x),float(y));
			counts[id] += 1.0f;
		}

	for(int i = 0 ;  i< n_node ; i ++ )
		ans[i] = ans[i] * (1.0/double( counts[i] ) );
	
	delete counts;

	return ans;
	
}

Mat LcGraphCut::getAdjancencyMatrix( Mat & label_map, int n_node)
{	
	Mat adj_mat = Mat::zeros( n_node, n_node, CV_32S);

	const int dx8[8] = {-1, -1,  0,  1, 1, 1, 0, -1};
	const int dy8[8] = { 0, -1, -1, -1, 0, 1, 1,  1};

	int _rows = label_map.rows;
	int _cols = label_map.cols;

	for(int y = 1; y< _rows-1; y++ )
		for(int x = 1 ; x< _cols-1; x++)
		{
			int id1 = label_map.at<int>(y,x);

			for( int k = 0 ; k < 8; k++)
			{
				int x2 = x + dx8[k];
				int y2 = y+ dy8[k];

				int id2 = label_map.at<int>( y2, x2);

				if(id1!=id2 && adj_mat.at<int>(id1,id2)==0)
				{
					adj_mat.at<int>(id1,id2)=1;
					adj_mat.at<int>(id2,id1)=1;				
				}
			}
		}
	
	return adj_mat;

}


Mat LcGraphCut::showSLIC( Mat & src, Mat & label_map )
{

	Mat dsp = Mat::zeros(src.rows,src.cols,src.type());	

	SLIC segment;

	int width = src.cols;
	int height = src.rows;
	int sz = width*height;

	unsigned int * pbuff = new(unsigned int[sz]);

	uchar * p = src.ptr<uchar>();

	for(int i = 0;i<sz;i++)
	{
		pbuff[i] = ((*p)<<16) | ((*(p+1))<<8) | *(p+2);
		p+=3;
	}

	int * klabels = new(int[sz]);

	int * p_label_map = label_map.ptr<int>();
	int * p_klabels = klabels;

	for(int i = 0;i<sz;i++,p_label_map++,p_klabels++)
	{
		(*p_klabels) = *p_label_map;
	}

	segment.DrawContoursAroundSegments(
		pbuff,
		klabels,
		width,
		height,		(((255)<<16) | ((255)<<8) | 255));

	p = dsp.ptr<uchar>();

	for(int i = 0;i<sz;i++)
	{
		unsigned int val = pbuff[i];
		*(p+2) = val % 256;
		*(p+1) = (val>>8) % 256;
		* p = val>>16;
		p+=3;
	}

	return dsp;
}

void LcGraphCut::getSLIC( Mat & src, Mat & label_map )
{
	uchar * p = src.ptr<uchar>();

	double m = compact_parameter;//Compactness factor. use a value ranging from 10 to 40 depending on your needs. Default is 10
	int k = n_superpixel;

	int width = src.cols;
	int height = src.rows;

	int sz = width*height;

	unsigned int * pbuff = new(unsigned int[sz]);

	int * klabels = NULL;

	int numlabels(0);

	for(int i = 0;i<sz;i++)
	{
		pbuff[i] = ((*p)<<16) | ((*(p+1))<<8) | *(p+2);
		p+=3;
	}

	SLIC segment;
	segment.DoSuperpixelSegmentation_ForGivenNumberOfSuperpixels(pbuff, width, height, klabels, numlabels, k, m);

	n_node = numlabels;

	label_map = Mat::zeros( src.size(), CV_32S);

	int * p_label_map = label_map.ptr<int>();
	int * p_klabels = klabels;

	for(int i = 0;i<sz;i++,p_label_map++,p_klabels++)
	{
		*p_label_map = (*p_klabels);
	}

	delete pbuff;
	delete klabels;
}







void LcGraphCut::mergeLikeli( Mat & frm, Mat & likelihood, char * ch, Mat & dst)
{
	vector< Mat > channels;

	channels.resize(3);

	for(int i = 0 ; i <3;i++)
	{
		if( ch[i] == 'l' ) likelihood.copyTo(channels[i]);
		else lc::getChannel( frm, channels[i], ch[i] );
	}
	
	cv::merge( channels, dst);
}

//======================================================
//==========NodeFeatureExtractor Family=================

void LcGraphCut::computeAllNode( Mat & desc )
{
	desc = Mat::zeros( n_node, dim_node_feature, 5);

	for(int i = 0 ; i < n_node ; i++)
	{
		int d = 0;
		for(int j= 0 ; j < (int)node_computer.size(); j++)
		{
			int _dim = node_computer[j]->dim;
			Mat _desc = desc(Rect(d,i, _dim ,1));

			node_computer[j]->compute( node_statistic[i] , _desc);

			d+=_dim;
		}
	}
}

void LcGraphCut::setNodeComputer()
{
	node_computer.clear();

	node_computer.push_back( new SpMeanLikeli );

	node_computer.push_back( new SpPositionPrior );

	//node_computer.push_back( new SpColorMoment );

	dim_node_feature = getDimNode();
}

int LcGraphCut::getDimNode()
{
	int ans = 0;

	for(int i = 0 ; i < (int) node_computer.size() ; i++ )
		ans +=node_computer[i]->dim;

	return ans;
}

void SpMeanLikeli::compute( NodeStatistic & node, Mat & desc)
{
	double val = node.sum_likelihood / (node.n+1e-3);
	if(val > 0.99) val = 0.99;
	else if( val < 0.01 ) val = 0.01;

	val = log(val / (1-val));

	desc.at<float>(0,0) = float(val);
}

void SpPositionPrior::compute( NodeStatistic & node, Mat & desc)
{
	double val = node.sum_position_prior / (node.n+1e-3);
	if(val > 0.99) val = 0.99;
	else if( val < 0.01 ) val = 0.01;

	val = log(val /( 1-val ));

	desc.at<float>(0,0) = float(val);
	
}


void SpColorMoment::compute( NodeStatistic & node, Mat & desc)
{
	Mat mu, cov;

	decodeMoment( node.color_moment, mu , cov );

	for(int i = 0 ; i < 3 ; i++ )
		desc.at<float>(0,i)  = mu.at<float>(0,i);

	for(int i = 0,k = 3 ; i < 3; i++)
		for(int j = i ; j < 3 ; j ++,k++)
			desc.at<float>(0,k) = cov.at<float>( i ,j);

	desc = min(desc, 5);
	desc = max(desc, -5);
}



//======================================================
//==========EdgeFeatureExtractor Family=================

void LcGraphCut::computeAllEdge( Mat & edge_map, Mat & desc )
{
	int n_edge = 0;
	for(int i = 0 ; i < n_node-1;i++)
		for(int j = 0 ; j < n_node; j++)
			if(adj_mat.at<int>(i,j) > 0 )
				n_edge++;

	edge_map = Mat::zeros( n_edge, 2, CV_32S);
	desc = Mat::zeros( n_edge, dim_edge_feature, 5);

	EdgeStatistic empty;

	n_edge = 0;
	for(int i = 0 ; i < n_node-1;i++)
		for(int j = 0 ; j < n_node; j++)
			if(adj_mat.at<int>(i,j) > 0 && i!=j)
			{
				int d = 0;

				for(int k = 0 ; k< (int)edge_computer.size();k++)
				{
					edge_map.at<int>(n_edge, 0) = i;
					edge_map.at<int>(n_edge, 1) = j;

					int _dim = edge_computer[k]->dim;

					Mat _desc = desc(Rect(d,n_edge, _dim ,1));

					if(edge_statistic[i][j]==NULL)
					{
						edge_computer[k]->compute( node_statistic[i] ,
							node_statistic[j], empty, _desc);
					}
					else
					{
						edge_computer[k]->compute(
							node_statistic[i] ,
							node_statistic[j], 
							*edge_statistic[i][j],
							_desc);
					}

					d+=_dim;
					
				}

				n_edge++;
			}
			
}

void LcGraphCut::setEdgeComputer()
{
	edge_computer.clear();

	edge_computer.push_back(new SpPb);

	edge_computer.push_back(new SpColorDiff);

	//edge_computer.push_back(new SpGaussTest);

	//edge_computer.push_back(new SpIndTTest);	
	
	dim_edge_feature = getDimEdge();
}


int LcGraphCut::getDimEdge()
{
	int ans = 0;

	for(int i = 0 ; i < (int) edge_computer.size() ; i++ )
		ans +=edge_computer[i]->dim;

	return ans;
}

void SpPb::compute(	NodeStatistic & node1,
					NodeStatistic & node2,
					EdgeStatistic & edge,
					Mat & desc)
{
	desc.at<float>(0,0) = max(0.0f,0.3f - edge.sum_Pb / (edge.len+1e-3f));
}

void SpColorDiff::compute(	NodeStatistic & node1,
							NodeStatistic & node2,
							EdgeStatistic & edge,
							Mat & desc)
{
	Mat mu1, cov1, mu2, cov2;

	decodeMoment( node1.color_moment, mu1, cov1 );
	decodeMoment( node2.color_moment, mu2, cov2 );

	mu1.convertTo( mu1, 5); mu2.convertTo( mu2, 5);

	float* p1 = mu1.ptr<float>();
	float* p2 = mu2.ptr<float>();

	float* pa = desc.ptr<float>();

	for(int i = 0 ; i < 3; i ++, pa++, p1++, p2++)
	{
		*pa = 1.1f - fabs(*p1-*p2)/255.0f;
		if(*pa<1e-3f) *pa = 1e-3f;
	}
}

void SpGaussTest::compute(	NodeStatistic & node1,
							NodeStatistic & node2,
							EdgeStatistic & edge,
							Mat & desc)
{
	//Mardia test

	Mat mu1, cov1, mu2, cov2;

	decodeMoment( node1.color_moment, mu1, cov1 );
	decodeMoment( node2.color_moment, mu2, cov2 );

	Mat W = (cov1 * double(node1.n) + cov2 * double( node2.n))
		* ( 1.0/ (node1.n + node2.n -2 ) );	

	W = W + Mat::eye( 3,3, CV_64F) * 1e-2;

	Mat mu_diff = mu1 - mu2;

	Mat t2 = (node1.n * node2.n) / (node1.n + node2.n) * mu_diff.t()
		* W.inv() * mu_diff;

	double dim = 3.0;

	Mat F = ( (node1.n + node2.n - dim - 1.0)/(node1.n + node2.n -2 ) / dim) * t2;

	F.convertTo( F, 5 );

	desc.at<float>(0,0) = F.at<float>(0,0);

	desc = min(desc, 5);
	desc = max(desc, -5);
}



void SpIndTTest::compute(	NodeStatistic & node1,
							NodeStatistic & node2,
							EdgeStatistic & edge,
							Mat & desc)
{
	//t test in individual dimension

	Mat mu1, cov1, mu2, cov2;

	decodeMoment( node1.color_moment, mu1, cov1 );
	decodeMoment( node2.color_moment, mu2, cov2 );

	Mat mu_diff = mu1 - mu2;	

	double n1 = double( node1.n +1e-3);
	double n2 = double( node2.n +1e-3);
	
	for(int i = 0 ; i < 3;i++)
	{
		double sxx = sqrt(
			( n1 * cov1.at<double>( i,i) 
			  + n2 * cov2.at<double> (i,i) )
			  /(n1+n2 - 2.0 +1e-3 ) ) + 1e-3;

		double t = fabs( mu_diff.at<float>(0,i) ) / sxx / sqrt( 1/n1 + 1/n2 +1e-3);

		desc.at<float>(0,i) = float(1.5 - t);
	}

	desc = min(desc, 5);
	desc = max(desc, -5);
}